
#include <ctl_api.h>
#include <stdio.h>
#include "freeRTOS.h"
#include "task.h"
#include "Vector.h"

extern "C"
{
#include <string.h>
#include "main.h"
#include "Logomatic_V2.h"
#include "ADC.h"

#if LOG_DATA
#include "fat16.h"
#include "rootdir.h"
#include "sd_raw.h"
#endif
}

#define BUFFER_LENGTH       4
#define ACCEL_THRESHOLD     50.0f
#define MIN_THRESHOLD       10.0f
#define ERROR_THRESHOLD     5.0f

#define TASK_STACK_SIZE             300
#define PID_TASK_PRIORITY           4
#define LOGGING_TASK_PRIORITY       5
#define COUNTING_TASK_PRIORITY      6

const int TICKS_PER_REVOLUTION = 9586;    // number of ticks per revolution of the wheel
const float WHEEL_RADIUS = 3.9;           // radius of the wheel (in inches)

const float BRAKE_PROP  = 1.1f;
const float PUSH_PROP   = 0.9f;
float speed_prop = 2.0f;

float _speed = 0.0f;            // current speed (initial=0)

/***************************/
/* PID Control Variables   */
/***************************/
  float _percent_error = 0.0f;
  float error_percent[4];
  unsigned int pid_delay=0;
  unsigned int push_delay=0;
  
  float previous_error = 0.0f;
  float integral=0,derivative=0,error=0;
  float output = 0.0f;
  unsigned int duty = 0;
  unsigned int duty_stable = 0;
  bool stable=false;

static const int HIGH_COAST_TIME=90;
static const int NORMAL_COAST_TIME=60;
static const int LOW_COAST_TIME=30;
static int COAST_TIME=NORMAL_COAST_TIME;        // defined in 10ths of a second (50=5seconds)
static int coasting_time = 0;

static const int AUTO_TIME_DELAY  = 10;         // time after releasing FSR before auto mode kicks in (defined in 10ths of a second)
int DELAY_AFTER_MANUAL = 0;

static const int MANUAL_MODE  = 1;
static const int AUTO_MODE    = 2;
static const int OFF_MODE     = 0;
static int DRIVE_MODE = OFF_MODE;

bool push_detected = false;

/***************************/
/*Push Detection Variables */
/***************************/
  unsigned int cap_val=0;
  unsigned int prev_cap_val = 0;
  signed int accel_vals[BUFFER_LENGTH];
  float filtered_vals[BUFFER_LENGTH];
  float filtered_DD[BUFFER_LENGTH];


#if LOG_DATA
  struct fat16_file_struct* handle;
  char name[32];
  char _str1[50];      // used for writing to the uSD card
#endif

typedef struct
{
   uint32_t frequency;            // Blinking frequency
} sTaskParams_t, *spTaskParams_t;

typedef struct
{
   float Kp,          // proportional constant
         Ki,          // integral constant
         Kd;          // derivative constant
   float setpoint;    // setpoint for PID
   uint32_t frequency;// frequency for task
} sPIDParams_t, *spPIDParams_t;

sTaskParams_t CountingArgs = {
                            10    // Task Execution Frequency
                            };

sPIDParams_t PIDArgs =      {
                            10,   // Kp
                            6,    // Ki
                            2,    // Kd
                            0,    // initial speed setpoint
                            10    // Task Execution Frequency
                            };


extern "C"
{
  int           main              ( void );
  void          bootup            ( void );
  void          PllFeed           ( void );
  static void   initTimer1        ( void );

#if LOG_DATA
  void          card_init         ( void );
  void          log_string        ( char *buf );
#endif

}

xTaskHandle xCountingTaskHandle,
            xPIDTaskHandle;
static void   CountingTask        ( void *pvParameters );
static void   PIDTask             ( void *pvParameters );

#if LOG_DATA
  xTaskHandle xLoggingTaskHandle;
  sTaskParams_t LoggingArgs = {
                              10    // Task Execution Frequency
                              }; 
  static void   LoggingTask         ( void *pvParameters );
#endif

void          readSwitches        ( void );

void          vTimer1ISRWrapper   ( void ) __attribute__ ( ( naked ) );
void          vTimer1Handler      ( void ) __attribute__ ( ( noinline ) );
void          UNDEF_Routine       ( void ) __attribute__ ( ( interrupt ( "UNDEF" ) ) );

extern "C" void abort( void )
{
  while (1 == 1)
  {}
} // abort


extern "C" void bootup( void )
{
portENTER_CRITICAL();
{
    SCS=3;

    // Setting Multiplier and Divider values
    PLLCFG=0x25; // M = 6
    PllFeed();
    
    // Enabling the PLL */
    PLLCON=0x1;
    PllFeed();
    
#define PLOCK 0x400
    // Wait for the PLL to lock to set frequency
    while(!(PLLSTAT & PLOCK)) ;
    
    // Connect the PLL as the clock source
    PLLCON=0x3;
    PllFeed();
    
    // Enabling MAM and setting number of clocks used for Flash memory fetch (4 cclks in this case)
    //MAMTIM=0x3; //VCOM?
    MAMCR=0x2;
    MAMTIM=0x4; //Original
    
    // Setting peripheral Clock (pclk) to System Clock (cclk)
    VPBDIV=0x1;

    P0DIR |= (1<<STAT1)|(1<<STAT0);
    
    PINSEL1 |= (2<<18);  // AOUT is on P0.25 (logomatic P4)

    PINSEL0 = 0;
    P0DIR &= ~(3);
}
portEXIT_CRITICAL();
      
    LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);
}

extern "C" void PllFeed( void )
{
    PLLFEED=0xAA;
    PLLFEED=0x55;
}


extern "C" void initTimer1( void )
{
    PINSEL1 |= (3<<10);           // enable cap1.3 on p0.21 (pin P6 on logomatic v2)

    T1TCR = (1<<1);                             // disable and reset Timer0
    T1CTCR = (3<<2)|(3<<0);                     // count using CAP1.3 as the counter
    T1TCR = 1;                                  // enable the timer
}

#if LOG_DATA
extern "C" void card_init( void )
{
	if(!sd_raw_init()) //Attempt to init SD raw mode
                while (1);

	if(openroot()) //Attempt to open root directory
		while (1);
}

extern "C" void log_string( char *buf )
{
	int stringSize = strlen(buf);
	fat16_write_file(handle, (unsigned char*)buf, stringSize);
	sd_raw_sync();
}

void LoggingTask( void * pvParameters )
{
    sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);

    int count = 0;
    sprintf(name,"TCK%02d.csv",count);
    while(root_file_exists(name))
    {
        count++;
        sprintf(name,"TCK%02d.csv",count);
    }
    handle = root_open_new(name);  
    sd_raw_sync();    
    log_string((char *)"speed,setpoint,pid,speedprop\n");

    while(true)
    {
        sprintf(_str1,"%0.3f,%0.3f,%0.3f,%0.3f\n",_speed,PIDArgs.setpoint,output,speed_prop);
        log_string(_str1);
        vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
    }
}
#endif

void readSwitches( void )
{
    int pinval = P0PIN;
    if ( (pinval&LOW_COAST_PIN) )
        COAST_TIME = LOW_COAST_TIME;
    else
    {
        if ( (pinval&HIGH_COAST_PIN) )
            COAST_TIME = HIGH_COAST_TIME;
        else
            COAST_TIME = NORMAL_COAST_TIME;
    }

    if ( (pinval&MANUAL_PIN) )
        DRIVE_MODE = MANUAL_MODE;
    else
    {
        if ( (pinval&AUTO_PIN) )
            DRIVE_MODE = AUTO_MODE;
        else
            DRIVE_MODE = OFF_MODE;
    }
}

void CountingTask( void * pvParameters )
{
    sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);

    float temp_accel,temp_dd;
    float New_SetPoint = 0.0f;

    signed int diff[BUFFER_LENGTH];
    signed int doublediff[BUFFER_LENGTH];
    int prev_coast_time;
    int push_delay=0;

    for (int i =0;i<BUFFER_LENGTH;i++)
    {
        accel_vals[i]=0;
        filtered_vals[i] = 0;
        filtered_DD[i]=0;
        diff[i] = 0;
        doublediff[i]=0;
    }
    
//    portTickType xLastWakeTime;
//    xLastWakeTime = xTaskGetTickCount();

    while(true)
    {        
        prev_coast_time = COAST_TIME;

        readSwitches();

        if (COAST_TIME<prev_coast_time)   // turn off the motor if the coast switch decreased the coast time
        {
            push_detected = false;
            PIDArgs.setpoint = 0.0f;
        }
        
        temp_accel = 0.0f;
        temp_dd = 0.0f;
        for (int i=0;i<(BUFFER_LENGTH-1);i++)
        {
            diff[i] = diff[i+1];
            doublediff[i] = doublediff[i+1];
            filtered_vals[i] = filtered_vals[i+1];
            filtered_DD[i] = filtered_DD[i+1];
            temp_accel += diff[i];
            temp_dd += doublediff[i];
        }

        prev_cap_val=cap_val;

        portENTER_CRITICAL();
        {
            cap_val = T1TC;
            T1TCR = (1<<1);                      // reset and disable Timer0
            T1TCR = 1;                           // enable Timer0
        }
        portEXIT_CRITICAL();

        diff[BUFFER_LENGTH -1] = cap_val - prev_cap_val;
        doublediff[BUFFER_LENGTH -1] = diff[BUFFER_LENGTH-1] - diff[BUFFER_LENGTH-2];

        _speed = ( (float)cap_val * ((float)p_Args->frequency) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) );

        filtered_vals[BUFFER_LENGTH-1] = ((float)diff[BUFFER_LENGTH-1]+ temp_accel)/((float)BUFFER_LENGTH);
        filtered_DD[BUFFER_LENGTH-1] = ((float)doublediff[BUFFER_LENGTH-1]+ temp_dd)/((float)BUFFER_LENGTH);

        if ( DRIVE_MODE == AUTO_MODE )
        {
            if (DELAY_AFTER_MANUAL < AUTO_TIME_DELAY)   // FSR was pressed recently
            {
                LogomaticV2Stat(STAT1, LOGOMATIC_STAT_OFF);
                DELAY_AFTER_MANUAL++;
                New_SetPoint = 0.0f;
                PIDArgs.setpoint=0.0f;
            }
            else    // if FSR has been released for a certain amount of time
            {
                if ( DRIVE_MODE == AUTO_MODE )   // if in Auto Mode and check for a push
                {
                    LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);  
                   // if ( (vectorMax(filtered_vals) >= ACCEL_THRESHOLD) && vectorGT(filtered_vals,MIN_THRESHOLD) &&((diff[BUFFER_LENGTH -1])>0) )
                   if(PIDArgs.setpoint>0.0f)
                   {
                      if( (output/10.0f/PIDArgs.Ki/_speed) < (speed_prop*PUSH_PROP) )
                        push_detected = true;
                   
                   }
                   else
                   {                   
                     if(filtered_vals[BUFFER_LENGTH -1]>40 && filtered_DD[BUFFER_LENGTH -1]>0)
                      {
                          push_detected = true;
                      }
                   }
                    if ( push_detected )
                    {   
                        
                        if ( _speed < New_SetPoint )
                        {
                            if(New_SetPoint>PIDArgs.setpoint*1.05)
                            {
                              PIDArgs.setpoint = New_SetPoint;
                              coasting_time=0;
                              push_detected = false;
                              New_SetPoint = 0.0f;
                            }
                            else
                              {
                                push_detected = false;
                                New_SetPoint =0.0f;
                              }
                        }
                        else
                        {
                            New_SetPoint = _speed;
                            if ( New_SetPoint < 1.0f )
                                New_SetPoint = 0;
                        }
                    }
                }
            }
        }
        if ( DRIVE_MODE == OFF_MODE )
        {
            LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
            LogomaticV2Stat(STAT1, LOGOMATIC_STAT_OFF); 
            New_SetPoint = 0.0f;
            PIDArgs.setpoint = 0.0f;
            coasting_time = 0;
            push_detected = false;
        }

        vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
    }
}

void PIDTask( void * pvParameters )
{
    sPIDParams_t *  p_Args = reinterpret_cast<sPIDParams_t *>(pvParameters);

    float Prev_SetPoint = 1.0f;

    float Prev_Integral = 0.0f;

    float stable_SetPoint = 1.0f;

    float stable_Integral = 2.3f;

    float pre_speed;
    int _adcval;


    while(true)
    {
        if ( DRIVE_MODE != OFF_MODE )
        {
            _adcval = getADC();
            if ( _adcval > 25 )
            {
                push_detected = false;
                PIDArgs.setpoint = 0.0f;
                DELAY_AFTER_MANUAL = 0;
                _adcval += 400;
                if (_adcval>1023)
                    _adcval=1023;
                if (_adcval==400)
                    _adcval=0;
                DACR = (1<<16)|((_adcval)<<6);
                LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
                LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);  
            }
            else
            {
//                error = p_Args->setpoint - _speed;
               
        
                if ( Prev_SetPoint != p_Args->setpoint )
                {
//                    previous_error = p_Args->setpoint - _speed;
//                    //integral = (stable_Integral/stable_SetPoint) * p_Args->setpoint;
//                    integral = 2.3f * p_Args->setpoint;
//                    Prev_SetPoint = p_Args->setpoint;
//                    error = p_Args->setpoint - _speed;
//                    integral = integral + (error);
//                    derivative = (error - previous_error);
                    duty = 0;
                    output = 0;

                 //   error_percent[0]=100;
                  //  error_percent[3]=100;
                   // error_percent[2]=100;
                   // error_percent[1]=100;
//                    error_percent[0]=0;
//                    error_percent[3]=0;
//                     error_percent[2]=0;
//                    error_percent[1]=0;
                    stable=false;
                    pid_delay=0;
                    duty_stable=0;
//                    pre_speed=0;
                }
                if ( p_Args->setpoint > 0.0f )
                {
//                    _percent_error = (_speed-pre_speed)/(_speed+0.001)*100.0f;
//                     pre_speed=_speed;
//                    _percent_error = (p_Args->setpoint - _speed)/p_Args->setpoint*100.0f;
        
        
//                    if(pid_delay>2)
//                    {
//                        error_percent[0]=error_percent[1];
//                        error_percent[1]=error_percent[2];
//                        error_percent[2]=error_percent[3];
//                        error_percent[3]=_percent_error;
//                    }
                    if ( !stable )
                    {
//                        integral = integral + (error);
//                        derivative = (error - previous_error);
//                        previous_error = error;                    
//                        output = (p_Args->Kp * error) + (p_Args->Ki * integral) + (p_Args->Kd * derivative);
                        output = (p_Args->Ki * 2.3*p_Args->setpoint);
                        output *= 10.0f;
                        
//                        if(output/10.0f/p_Args->Ki/_speed>10)//(_speed>2.5 ? 2.65f:3.0f))
//                        {
//                            DACR = (1<<16)|(0<<6);
//                            push_detected = false;
//                            PIDArgs.setpoint = 0.0f;
//                            stable=false;
//                        }
//                        else
//                        {
                            duty = (output>0.0f) ? ((int)output) : ((int)0);
                            duty = (duty<(1023-400)) ? duty : (1023-400);
                            duty += 400;
                            if( duty<=400)
                                duty=0;                         
                            DACR = (1<<16)|(duty<<6);
                            pid_delay++;
                            coasting_time=0;
                            speed_prop = output/10.0f/p_Args->Ki/_speed;
//                        }
                    }
                    
                    //if ( vectorAbsLT(error_percent,ERROR_THRESHOLD) && !stable )
                    if(pid_delay>7) 
                    {
                        duty_stable=duty;
                        stable=true;
//                        stable_SetPoint=p_Args->setpoint;
//                        stable_SetPoint=_speed;
//                        stable_Integral=integral;    
                    }
                    else
                    {
                        if ( (output/10.0f/p_Args->Ki/_speed) > 3.3 )
                        {
                            DACR = (1<<16)|(0<<6);
                            push_detected = false;
                            PIDArgs.setpoint = 0.0f;
                            stable=false;
                        }
                  
                    }
                    if ( stable )
                    {
                        coasting_time++;
                        //if( (coasting_time==COAST_TIME)|| ((abs(error_percent[2])>10) && (abs(error_percent[3])>10) ))
                        //if( (coasting_time==COAST_TIME)|| ((abs(_percent_error)>10)))
                        if  ( (coasting_time==COAST_TIME) || ( (output/10.0f/p_Args->Ki/_speed) > (speed_prop*BRAKE_PROP) ) )
                        {
                            DACR = (1<<16)|(0<<6);
                            push_detected = false;
                            PIDArgs.setpoint = 0.0f;
                            stable=false;
                        }
                        else
                            DACR = (1<<16)|(duty_stable<<6);
                    }
                }
                else
                    DACR = (1<<16)|(0<<6);
            }
        }
        else
        {
            DACR = (1<<16)|(0<<6);
            push_detected = false;
            PIDArgs.setpoint = 0.0f;
            stable=false;
        }
        vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
    }
}

int main( void )
{
   // Initialize the logomatic board processor and preipherals
   bootup();
   initADC();
   initTimer1();

   // Create the Logging task
   xTaskCreate(CountingTask,
               reinterpret_cast<const signed char *>("TCK"),
               TASK_STACK_SIZE,
               &CountingArgs,
               COUNTING_TASK_PRIORITY,
               &xCountingTaskHandle
               );

   // Create the PID controller task
   xTaskCreate(PIDTask,
               reinterpret_cast<const signed char *>("PID"),
               TASK_STACK_SIZE,
               &PIDArgs,
               PID_TASK_PRIORITY,
               &xPIDTaskHandle
               );

#if LOG_DATA
   card_init();
   // Create the Logging task
     xTaskCreate(LoggingTask,
                 reinterpret_cast<const signed char *>("LOG"),
                 TASK_STACK_SIZE,
                 &LoggingArgs,
                 LOGGING_TASK_PRIORITY,
                 &xLoggingTaskHandle
               );
#endif

    LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
    LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);
   
   // Start the multitasking environment
   vTaskStartScheduler();
   
   return 0;
} // main
